using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Xna.Framework;

namespace Oops.Xna.Framework.Physics.CollisionDetectors
{
	/// <summary>
	/// Detects collisions between a <see cref="CollisionSphere"/> and a <see cref="CollisionMesh"/>.
	/// </summary>
	public class SphereMeshCollisionDetector : CollisionDetector
	{
		private static List<CollisionMeshTriangle> triangles = new List<CollisionMeshTriangle>(32);

		#region CollisionDetector Members

		public override void Detect(CollisionShape shape0, CollisionShape shape1, CollisionContext context)
		{
			// Typecast the shapes to their correct subclasses.
			CollisionSphere sphere = (CollisionSphere)shape0;
			CollisionMesh mesh = (CollisionMesh)shape1;

			// Attempt to get all triangles that intersect with the sphere's axis-aligned bounding box.
			SphereMeshCollisionDetector.triangles.Clear();
			if (mesh.GetIntersectingTriangles(ref sphere.AABB, SphereMeshCollisionDetector.triangles))
			{
				// Get radius totals.
				float radiusTotal = sphere.Radius + context.Tolerance;
				float radiusSquaredTotal = radiusTotal * radiusTotal;

				// Transform the sphere's position into the mesh's object space.
#if INLINE
#if XBOX
				Vector3 spherePosition = Vector3.Zero;
#else
				Vector3 spherePosition;
#endif
				spherePosition.X = sphere.Position.X * mesh.inverseTransform.M11 + sphere.Position.Y * mesh.inverseTransform.M21 + sphere.Position.Z * mesh.inverseTransform.M31 + mesh.inverseTransform.M41;
				spherePosition.Y = sphere.Position.X * mesh.inverseTransform.M12 + sphere.Position.Y * mesh.inverseTransform.M22 + sphere.Position.Z * mesh.inverseTransform.M32 + mesh.inverseTransform.M42;
				spherePosition.Z = sphere.Position.X * mesh.inverseTransform.M13 + sphere.Position.Y * mesh.inverseTransform.M23 + sphere.Position.Z * mesh.inverseTransform.M33 + mesh.inverseTransform.M43;
#elif REFERENCE
				Vector3 spherePosition;
				Vector3.Transform(ref sphere.Position, ref mesh.inverseTransform, out spherePosition);
#else
				Vector3 spherePosition = Vector3.Transform(sphere.Position, mesh.inverseTransform);
#endif
				// Initialize collision manifold.
				CollisionPointCollection collisionPoints = CollisionPointCollection.Retrieve();
				Vector3 collisionNormal = Vector3.Zero;

				// Iterate on the potentially intersecting triangles.
				for (int i = 0; i < SphereMeshCollisionDetector.triangles.Count; i++)
				{
					// Get the current triangle.
					CollisionMeshTriangle triangle = SphereMeshCollisionDetector.triangles[i];

					// Calculate the distance to the triangle's plane.
#if INLINE
					float distanceToPlane = (triangle.Normal.X * spherePosition.X + triangle.Normal.Y * spherePosition.Y + triangle.Normal.Z * spherePosition.Z) + triangle.distance;
#elif REFERENCE
					float distanceToPlane = Vector3Extension.Dot(ref triangle.Normal, ref spherePosition) + triangle.distance;
#else
					float distanceToPlane = Vector3.Dot(triangle.Normal, spherePosition) + triangle.distance;
#endif
					// Determine if the sphere is close enough to the plane of triangle for a collision.  Early out test.
					if (/*distanceToPlane <= 0.0f || */distanceToPlane >= radiusTotal)
					{
						continue;
					}

					// Get the closet point to the triangle.  This will be the collision position if
					// it's within the radius of the sphere.
#if REFERENCE
					Vector3 collisionPosition;
					triangle.GetClosestPoint(ref spherePosition, out collisionPosition);
#else
					Vector3 collisionPosition = triangle.GetClosestPoint(spherePosition);
#endif
					// Get the distance between the sphere center and the closest point to the triangle.
#if INLINE
					float dx = spherePosition.X - collisionPosition.X;
					float dy = spherePosition.Y - collisionPosition.Y;
					float dz = spherePosition.Z - collisionPosition.Z;
					float distanceSquared = dx * dx + dy * dy + dz * dz;
#elif REFERENCE
					float distanceSquared;
					Vector3.DistanceSquared(ref spherePosition, ref collisionPosition, out distanceSquared);
#else
					float distanceSquared = Vector3.DistanceSquared(spherePosition, collisionPosition);
#endif
					// Determine if the sphere is close enough to the triangle for a collision.
					if (distanceSquared < radiusSquaredTotal)
					{
						// Transform the collision position into world space.
#if INLINE
						float x = collisionPosition.X * mesh.transform.M11 + collisionPosition.Y * mesh.transform.M21 + collisionPosition.Z * mesh.transform.M31 + mesh.transform.M41;
						float y = collisionPosition.X * mesh.transform.M12 + collisionPosition.Y * mesh.transform.M22 + collisionPosition.Z * mesh.transform.M32 + mesh.transform.M42;
						float z = collisionPosition.X * mesh.transform.M13 + collisionPosition.Y * mesh.transform.M23 + collisionPosition.Z * mesh.transform.M33 + mesh.transform.M43;
#elif REFERENCE
                        Vector3.Transform(ref collisionPosition, ref mesh.transform, out collisionPosition);
#else
						collisionPosition = Vector3.Transform(collisionPosition, mesh.transform);
#endif
						// Calculate the depth of the penetration due to the collision.
						float distance = (float)Math.Sqrt(distanceSquared);
						float collisionDepth = sphere.Radius - distance;
												
						// Determine the normal of the collision.
						Vector3 normal = triangle.Normal;

						// Create the collision point.
						CollisionPoint point = CollisionPoint.Retrieve();
#if INLINE
						point.RelativePosition0.X = x - shape0.CollisionGroup.Position.X;
						point.RelativePosition0.Y = y - shape0.CollisionGroup.Position.Y;
						point.RelativePosition0.Z = z - shape0.CollisionGroup.Position.Z;

						point.RelativePosition1.X = x - shape1.CollisionGroup.Position.X;
						point.RelativePosition1.Y = y - shape1.CollisionGroup.Position.Y;
						point.RelativePosition1.Z = z - shape1.CollisionGroup.Position.Z;
#elif REFERENCE
						Vector3.Subtract(ref collisionPosition, ref shape0.CollisionGroup.Position, out point.RelativePosition0);
						Vector3.Subtract(ref collisionPosition, ref shape1.CollisionGroup.Position, out point.RelativePosition1);
#else
						point.RelativePosition0 = collisionPosition - shape0.CollisionGroup.Position;
						point.RelativePosition1 = collisionPosition - shape1.CollisionGroup.Position;
#endif
						point.Depth = collisionDepth;

						// Add the collision point to the collection.
						collisionPoints.Add(point);

						// Combine the normal of this collision to the total.
#if INLINE
						collisionNormal.X = collisionNormal.X + normal.X;
						collisionNormal.Y = collisionNormal.Y + normal.Y;
						collisionNormal.Z = collisionNormal.Z + normal.Z;
#elif REFERENCE
						Vector3.Add(ref collisionNormal, ref normal, out collisionNormal);
#else
						collisionNormal += normal;
#endif
					}
				}

				if (collisionPoints.Count > 0)
				{
					// Unitize the combined collision normal.
					Vector3Extension.SafeNormalize(ref collisionNormal);

					// Transform the collision normal into world space.
#if INLINE
                    float x = collisionNormal.X * mesh.transform.M11 + collisionNormal.Y * mesh.transform.M21 + collisionNormal.Z * mesh.transform.M31;
                    float y = collisionNormal.X * mesh.transform.M12 + collisionNormal.Y * mesh.transform.M22 + collisionNormal.Z * mesh.transform.M32;
                    float z = collisionNormal.X * mesh.transform.M13 + collisionNormal.Y * mesh.transform.M23 + collisionNormal.Z * mesh.transform.M33;
                    collisionNormal.X = x;
                    collisionNormal.Y = y;
                    collisionNormal.Z = z;
#elif REFERENCE
                    Vector3.TransformNormal(ref collisionNormal, ref mesh.transform, out collisionNormal);
#else
					collisionNormal = Vector3.TransformNormal(collisionNormal, mesh.transform);
#endif
					// Notify subscribers of the collision.
					context.OnCollided(sphere, mesh, ref collisionNormal, collisionPoints);
				}

				// Recycle the collision point collection and any triangles found.
				CollisionPointCollection.Recycle(collisionPoints);
				mesh.ReleaseIntersectingTriangles(SphereMeshCollisionDetector.triangles);
			}
		}

		#endregion
	}
}